#ifndef Q_MOC_RUN

#include <cmath>
#include <ros/ros.h>
#include <rviz/panel.h>
#include "tf/transform_datatypes.h"

#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>

#include "rviz/properties/bool_property.h"
#include "rviz/properties/string_property.h"

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <nav_msgs/Path.h>
#include "path_creator_rviz_plugin/LoadPaths.h"
#include "path_creator_rviz_plugin/SavePaths.h"
#include "path_creator_rviz_plugin/my_path.h"

#define PI 3.14159265  

#endif

class QPushButton;
class QLineEdit;

namespace path_creator_rviz_plugin
{
    class rvizPathPanel: public rviz::Panel
    {
    Q_OBJECT
    public:
        rvizPathPanel( QWidget* parent = 0 );

    protected:
        //markers to visualize all paths
        visualization_msgs::MarkerArray paths;

        // Buttons
        //click on this button to remove path by id from string "id"
        QPushButton* remove_path_by_id_button;
        //click on this button to create new path
        QPushButton* create_new_path_button;
        //click on this button to create new path
        QPushButton* save_path_button;
        //click on this to show all paths from file
        QPushButton* show_all_paths_button;

        // One-line text editor for entering the path to file
        QLineEdit* path_to_file_editor;
        // One-line text editor for entering the path to file
        QLineEdit* path_id_editor;
        // One-line text editor for entering the path's description
        QLineEdit* path_description_editor;

        std::string path_to_file;
        std::string frame_id;
        std::string path_description;

        ros::Publisher path_creating_pub;
        ros::Publisher markers_pub;
        ros::Publisher last_path_pub;
        ros::Subscriber pose_received;
        ros::ServiceClient load_paths_client;
        ros::ServiceClient save_paths_client;

        ros::NodeHandle nh_;

        void receivedNewPose(const geometry_msgs::Pose::ConstPtr& msg);
    
    protected Q_SLOTS:
        //buttons
        void removePathById(bool trig);
        void showAllPaths(bool trig);
        void createNewPath(bool trig);
        void savePathToFile(bool trig);
        //editors
        void updatePathToFile();
        void updatePathId();
        void updatePathDescription();

    private:
        //fields
        visualization_msgs::MarkerArray marker_array_paths;
        std::list<path_creator_rviz_plugin::my_path> paths_list;
        nav_msgs::Path current_path;
        float step_for_interpolation;
        int current_path_id;
        int map_id;
        double line_width;

        //methods
        void displayAllPaths(visualization_msgs::MarkerArray marker_array_);
        visualization_msgs::MarkerArray pathsToMarkersArray(std::list<path_creator_rviz_plugin::my_path> paths_list_);
        visualization_msgs::Marker transformPathToMarker(nav_msgs::Path path);
        visualization_msgs::Marker transformMyPathToIdLabelMarker(geometry_msgs::PoseStamped pose_stamped, int path_id);
        std::vector<geometry_msgs::PoseStamped> interpolationBetweenTwoPoses(geometry_msgs::PoseStamped pose_prev, geometry_msgs::PoseStamped pose_new);
        float angleBetweenPoints(geometry_msgs::Pose pose_prev, geometry_msgs::Pose pose_new);
        void clearAllMarkers();
        void removePathById(int path_id);
        bool pathIsAlreadyExist(int path_id);
        bool pathIsAlreadyExist(std::string description);        
        void loadPaths (int map_id);
    };

} // namespace rviz_pose_send_tool
